/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: CoordinateDrive_Dummy.cpp,v 1.18 2009-10-22 12:49:26 morsko Exp $
 *
 * \file
 * \brief Dummy implementation for CoordinateDrive Server interface.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 * This implementation can act as an example / codebase for any
 * CoordinateDrive server based module implementation. This code follows a
 * pseudogood coding style, and also provides a basic parameter
 * parser that can be directly used with slight modifications.
 * 
 */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "owndebug.h"
#include "CoordinateDriveServer.hpp"
#include "binbag.h"

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
static int verbose = 1;
//*****************************************************************************
using namespace MaCI::CoordinateDrive;
//*****************************************************************************

void showUsage()
{
  fprintf(stderr,"\n");
  fprintf(stderr,"Usage:\tSpeedCtrl_Dummy [OPTIONS]\n");
  fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
  fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
  fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
  fprintf(stderr,"  [-G name]\t\t\tUse 'name' as MaCIGroup for this service\n");
  fprintf(stderr,"  [-I name]\t\t\tUse 'name' as Instancename for this service\n");

  fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
  fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
  fprintf(stderr, "\n\n");
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

int main(int argc, char *argv[])
{
 // GIMnet parameters
  std::string gimnetAP = "asrobo.hut.fi";
  int gimnetAPPort = 50002;
  std::string gimnetName = "";
  std::string maciGroupName;
  std::string interfaceInstanceName = "Dummy";
  // Objects
  bool run = true;

  std::vector<TCoordinate2D> coordinates;


  gimi::GIMI g;
  MaCI::MaCICtrl::CMaCICtrlServer msc(&g);
  MaCI::CoordinateDrive::CCoordinateDriveServer cdS(&msc, 0); 

  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(4);
  debugSetLogFilename("CoordinateDrive_Dummy.log");

  // Greet
  fprintf(stderr,
	  "\nCoordinateDrive_Dummy - MaCI CoordinateDrive Interface implementation for Dummy - v0.00001\n");

  // Parse parameters
  bool parse = true;
  while(parse) {
    int c;
    c = getopt(argc, argv, "I:G:n:u:p:hqv");
    switch(c) {

      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////

      /////////////////////////////////
      ///// GIMnet parameters /////////
      /////////////////////////////////
    case 'n':
      gimnetName = optarg;
      dPrint(3,"Using GIMnet Name '%s'", optarg);
      break;

    case 'u':
      gimnetAP = optarg;
      dPrint(3,"Using GIMnetAP address '%s'", optarg);
      break;

    case 'p':
      gimnetAPPort = atoi(optarg);
      dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
      break;
 
    case 'G':
      maciGroupName = optarg;
      dPrint(3,"Using MaCIGroupName '%s'", optarg);
      break;

    case 'I':
      interfaceInstanceName = optarg;
      dPrint(3,"Using InterfaceInstance Name '%s'", optarg);
      break;

      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////
    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0)
	debugSetGlobalDebugLvl(++verbose);
      break;
      
    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(0);
      break;
      
    case 'h':
    case '?':
      showUsage();
      exit(1);
      break;
      
    case -1:
      parse = false;
      break;
    }
  }

  msc.SetGroupName(maciGroupName);
  cdS.SetInterfaceInstanceName(interfaceInstanceName);
  int r;
  if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
    MaCI::EMaCIError e;
    if ( (e = cdS.Open()) != MaCI::KMaCIOK) {
      dPrint(1,"Failed to Open SpeedCtrlServer instance! (%s)",
	     GetErrorStr(e).c_str());
      
    } else {
      ////// Execute test section ///////

      // Run as long as 'run' is set.
      bool sendEvent = false;
      float drivingSpeed = 0.1, drivingAngularSpeed =1.1;

      int loop = 0;
      while (run) {
	using namespace MaCI::CoordinateDrive;

	// Construct Data element
	CCoordinateDriveData cdData;
	gim::binbag::CBinBag bb;
	TReplyInfo rInfo(0,0);
	// Now, sit back and wait for command to arrive (for 1000ms)
        e = cdS.WaitCommand(cdData, rInfo,1000);

	// Got command? (No timeout or error)
        if (e == MaCI::KMaCIOK) {
	  dPrint(1,"Got command");
	  const TCommand *cmd = cdData.GetCommand();

	  cdData.Print(1);
	  assert(cmd != NULL);
	  // Switch based on command type
	  switch(cmd->cmd) {
	    
	  case KCommandSetCoordinate2D:{
	    const TCoordinate2D *coord = cdData.GetCoordinate(0);
	    if(coord){
	      dPrint(1,"Got Command - SetCoordinate2D = (%f,%f,%f), (%d,%d,%d)",
		     coord->x,
		     coord->y,
		     coord->a,
		     coord->isDifferential,
		     coord->isPathTarget,
		     coord->isWaitPoint);
              coordinates.push_back(*coord);



              //SEnd event of coordinates
              CCoordinateDriveData data;
              gim::binbag::CBinBag bb2;
              const TCoordinateHeader header(coordinates.size());
              data.EncodeTo(&bb2);
              data.SetCommand(TCommand(KCommandCoordinatesEvent));
              data.SetTimestamp(MaCI::Common::TTimestamp());
              data.SetCoordinateHeader(header);
              for (unsigned int i = 0; i< coordinates.size();i++){
                data.SetCoordinate2D(coordinates[i]);
              }
              if(cdS.SendCoordinatesEvent(data)){
                dPrint(1,"Succesfully sent coordinateDrive point event");
              }else{
                dPrint(1,"Failed to send coordinateDrive point event");
              }

              if(coordinates.size() > 3){
                sendEvent = true;
                coordinates.erase(coordinates.begin());
              }
	    }else{
	      dPrint(1,"Coord == NULL");
	    }
	    
	    break;
	  }

	  case KCommandRemoveCoordinates:{
	    dPrint(1,"Got Command - Remove Coordinates");
            coordinates.clear();
            CCoordinateDriveData data;
              gim::binbag::CBinBag bb2;
              const TCoordinateHeader header(0);
              data.EncodeTo(&bb2);
              data.SetCommand(TCommand(KCommandCoordinatesEvent));
              data.SetTimestamp(MaCI::Common::TTimestamp());
              data.SetCoordinateHeader(header);
              cdS.SendCoordinatesEvent(data);
	    //Remove all the normal target points
	   
	    break;
	  }

	  case KCommandSetStart:{
	    dPrint(1,"Got Command - Set Start");
	    break;
	  }

	  case KCommandSetStop:{
	    dPrint(1,"Got Command - Set Stop");
	    break;
	  }

	  case KCommandSetDriveMode:{
	    const TDriveMode *dMode = cdData.GetDriveMode(0);
	    dPrint(1,"Got Command - Set DriveMode = %d",
		   dMode->mode);
	    break;
	  }

	  case KCommandUnsetDriveMode:{
	    const TDriveMode *dMode = cdData.GetDriveMode(0);
	    dPrint(1,"Got Command - Unset DriveMode = %d",
		   dMode->mode);
	    break;
	  }

	  case KCommandContinue:{
	    dPrint(1,"Got Command - Continue");
	    break;
	  }

	  case KCommandGetCoordinates:{
            //SEnd event of coordinates
              CCoordinateDriveData data;
              gim::binbag::CBinBag bb2;
              const TCoordinateHeader header(coordinates.size());
              data.EncodeTo(&bb2);
              data.SetCommand(TCommand(KCommandGetCoordinatesReply));
              data.SetTimestamp(MaCI::Common::TTimestamp());
              data.SetCoordinateHeader(header);
              for (unsigned int i = 0; i< coordinates.size();i++){
                data.SetCoordinate2D(coordinates[i]);
              }
              if(cdS.SendReply(data,rInfo) == MaCI::KMaCIOK){
                dPrint(1,"Succesfully sent coordinateDrive point reply");
              }else{
                dPrint(1,"Failed to send coordinateDrive point reply");
              }
	   
	    
	    break;
	  }

	  case KCommandGetDrivingModes:{
	    //Construct the header, telling there are 2 driveMode-structs after this header
	    const TDriveModeHeader header(2);
	    //Construct the CCoordinateDriveData
	    CCoordinateDriveData replyData;
	    //Encode it, set command and the header 
	    replyData.EncodeTo(&bb);
	    replyData.SetCommand(TCommand(KCommandGetDrivingModesReply));
	    replyData.SetDriveModeHeader(header);
	    //Construct the DriveModes and Set them
	    const TDriveMode mode1( KModeTurnFirstTowardGoal);
	    const TDriveMode mode2( KModePathDriving);
	    replyData.SetDriveMode(mode1);
	    replyData.SetDriveMode(mode2);
	    //Send the reply message
	    e = cdS.SendReply(replyData,rInfo);
	    if(e != MaCI::KMaCIOK){
	      dPrint(1,"WARNING: Reply sending failed!");
	    }
	    break;
	    
	  }

          case KCommandGetPlanFromTo:{

            CCoordinateDriveData replyData;
            //Encode it, set command and the header 
	    replyData.EncodeTo(&bb);
            replyData.SetCommand(TCommand(KCommandGetPlanFromToReply));
            replyData.SetCoordinateHeader(TCoordinateHeader(11));
            
            const TCoordinate2D *cFrom = cdData.GetCoordinate(0);
            const TCoordinate2D *cTo = cdData.GetCoordinate(1);

            float stepX = (cTo->x - cFrom->x)/10.0;
            float stepY = (cTo->y - cFrom->y)/10.0;

            for(int i = 0; i< 11; i++){

              replyData.SetCoordinate2D(TCoordinate2D(cFrom->x + stepX*i,
                                                      cFrom->y + stepY*i,
                                                      cFrom->a,
                                                      cFrom->isDifferential,
                                                      cFrom->isPathTarget,
                                                      cFrom->isWaitPoint));
            }
            e = cdS.SendReply(replyData,rInfo);
	    if(e != MaCI::KMaCIOK){
	      dPrint(1,"WARNING: Reply sending failed!");
	    }
            
            break;
          }

          case KCommandGetMaxDrivingSpeed:{
            CCoordinateDriveData replyData;
            //Encode it, set command and the header 
	    replyData.EncodeTo(&bb);
            replyData.SetCommand(TCommand(KCommandGetMaxDrivingSpeedReply));
            replyData.SetSpeed(TSpeed(drivingSpeed,drivingAngularSpeed));
            e = cdS.SendReply(replyData,rInfo);
	    if(e != MaCI::KMaCIOK){
	      dPrint(1,"WARNING: Reply sending failed!");
	    }
            break;
          }

          case KCommandSetDrivingSpeed:{
            const TSpeed *speeds = cdData.GetSpeed();
            if(speeds){
              dPrint(1,"Got Command - Set Driving Speed = %f,%f",
                     speeds->speed,
                     speeds->angularSpeed);
              drivingSpeed = speeds->speed;
              drivingSpeed = speeds->angularSpeed;
            }else{
              dPrint(1,"Got Command - Set Driving Speed, but no speed container ! :(");
            }
	    break;

          }
	  default:{
	    dPrint(1,"No implementation for command %d!", cmd->cmd);
	    break;
	  }
	  }

        } else if (e == MaCI::KMaCITimeout) {
          // No, timeout waiting for command. Print and wait again.
          dPrint(8,"Timeout waiting for Command");
                 
        } else {
          // Quck? Some error occured? Print it, sleep an extra second and try again.
          dPrint(1,"Failed to WaitCommand()! (%s)",
                 GetErrorStr(e).c_str());
          ownSleep_ms(1000);
        }
      
	if(sendEvent){

	  dPrint(1,"Sleeping for a sec before sending back arrived to point event...");
	  ownSleep_ms(1000);
	  dPrint(1,"Sending");
          CCoordinateDriveData replyData;
          replyData.EncodeTo(&bb);
	  replyData.SetCommand(TCommand(KCommandArrivedToPointEvent));
          replyData.SetCoordinate2D(coordinates[0]);
          replyData.Print(1);
	  cdS.SendArrivedToTargetEvent(replyData);
	  sendEvent = false;

	  dPrint(1,"..Sent");
          dPrint(1,"Sending coordinates");
          //SEnd event of coordinates
          CCoordinateDriveData data;
          gim::binbag::CBinBag bb2;
          const TCoordinateHeader header(coordinates.size());
          data.EncodeTo(&bb2);
          data.SetTimestamp(MaCI::Common::TTimestamp());
          data.SetCommand(TCommand(KCommandCoordinatesEvent));
          data.SetCoordinateHeader(header);
          for (unsigned int i = 0; i< coordinates.size();i++){
            data.SetCoordinate2D(coordinates[i]);
          }
          cdS.SendCoordinatesEvent(data);
          dPrint(1,"Sent");
	}

        // NOTE: THIS IS JUST FOR TESTING SENDING STATUS EVENTS
        //       DO _NOT_ COPY PASTE THIS TO YOUR PROGRAM
        //       DO THE SAME SOMEHOW SANELY :)
        //       These notifications are used in different kinds of planners
        //       UnableToDriveToCoordinate can be used also in normal coordinatedrive.
        if(loop %10 == 0){


          if(loop%30 == 0){
            dPrint(1,"SENDING SERVER STATUS: UNABLE TO DRIVE TO COORDINATE");
            TCoordinate2D coord(loop,loop,loop,0,0,0);
            cdS.UnableToDriveToCoordinate(coord);
          }else if(loop%40 == 0){
          dPrint(1,"SENDING SERVER STATUS: CALCULATING");
            cdS.SendCalculatingEvent();
          }else{
          dPrint(1,"SENDING SERVER STATUS: PATH READY");
            cdS.SendPathReadyEvent();

          }

        }
        loop++;
      }
    }
  }

  return 0;
} 

//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
